import rclpy                        # ROS2 Python接口库
from rclpy.node import Node         # ROS2 节点类
from recognition_interface.srv import GetPedPos
from cv_bridge import CvBridge
import cv2
import os

OUTPUT_DIR = os.environ.get("ROS_WS")+"/output/"

class GetPosClient(Node):
    def __init__(self, name):
        super().__init__(name)
        self.client = self.create_client(GetPedPos, "GetPedPos")
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info("service is not avaliable, waiting again...")

        self.request = GetPedPos.Request()
        self.capture = cv2.VideoCapture(0)
        self.bridge = CvBridge()

    def send_request(self):
        ret, img = self.capture.read()
        if not ret:
            self.get_logger().info(f"failed capturing picture")
            return
        self.get_logger().info(f"capturing picture...")
        self.request.imgmsg = self.bridge.cv2_to_imgmsg(img, encoding='bgr8')
        self.future = self.client.call_async(self.request)

    def back_feed(self)->bool:
        info = self.get_logger().info
        if self.future.done():
            response = self.future.result()
            info("Recognized %d person" % len(response.rectangles))

            image = self.bridge.imgmsg_to_cv2(self.request.imgmsg)
            for rec in response.rectangles:
                x, y, w, h = rec.side
                pad_w, pad_h = int(0.15 * w), int(0.01 * h)
                point = ((x+x+w)>>1, (y+y+h)>>1)
                cv2.rectangle(image, (x + pad_w, y + pad_h), (x + w - pad_w, y + h - pad_h), (0, 255, 0), 2)
                cv2.circle(image, point, 5, (0,0,255), -1)
                
            cv2.imshow('recognized', image)
            cv2.waitKey(1)

            self.send_request()
            return True
        return True
    
def main(args=None):
    rclpy.init(args=args)
    node = GetPosClient("GetPedPos_client")
    node.send_request()

    while rclpy.ok():
        rclpy.spin_once(node)
        state = node.back_feed()
        if not state:
            break
    
    node.destroy_node()
    rclpy.shutdown()

if __name__=="__main__":
    main()